
open Chipmunk.Low_level

module Segment = struct
	type t = cpShape * cpSegmentShape

	let create space start_x start_y end_x end_y =
		let body = cpBodyNew infinity infinity in
		let shape = cpSegmentShapeNew ~body ~a:(cpv start_x start_y) ~b:(cpv end_x end_y) ~r:0.0 in
		let () = cpShapeSetElasticity ~shape ~e:1.0 in	
		let () = cpShapeSetFriction ~shape ~u:1.0 in
		let () = cpSpaceAddStaticShape ~space ~shape in
		(shape, cpSegmentShape_of_cpShape shape)

	let show (shape, segment) = 
		let body = cpShapeGetBody ~shape in
		let relative1 = cpSegmentShapeGetA segment in
		let relative2 = cpSegmentShapeGetB segment in
		let absolute1 = cpvadd (cpBodyGetPos ~body)
			(cpvrotate (relative1) (cpBodyGetRot ~body)) in
		let absolute2 = cpvadd (cpBodyGetPos ~body)
			(cpvrotate (relative2) (cpBodyGetRot ~body)) in
		Types.Data.List [
			Types.Data.String "Segment";
			Types.Data.Float absolute1.cp_x;
			Types.Data.Float absolute1.cp_y;
			Types.Data.Float absolute2.cp_x;
			Types.Data.Float absolute2.cp_y]
		
	let draw data = match data with
		| Types.Data.List [
			Types.Data.String "Segment";
			Types.Data.Float a_x;
			Types.Data.Float a_y;
			Types.Data.Float b_x;
			Types.Data.Float b_y] ->
				let () = GL.glBegin GL.GL_LINES in
					let () = GL.glColor3 1.0 1.0 1.0 in
					let () = GL.glVertex2 a_x a_y in
					let () = GL.glVertex2 b_x b_y in
				let () = GL.glEnd() in
				()
		| _ -> assert false
		 	
	let step shape = ()
end

module Box = struct
	type t = cpBody * float (* body, size *)

	let genVerts center_x center_y size rot = 
		Array.map 
			(fun v -> cpvadd (cpv center_x center_y) (cpvrotate v rot))
			[|cpv ~x:size ~y:size; 
			  cpv ~x:(size) ~y:(-.size);
			  cpv ~x:(-.size) ~y:(-.size); 
			  cpv ~x:(-.size) ~y:(size) |]
				
	let create space center_x center_y size mass =
        let size' = size *. Config.cell_size *. 0.5 in
		let verts = genVerts 0.0 0.0 size' (cpv ~x:1.0 ~y:0.0) in
		let moment = cpMomentForPoly mass verts (cpvzero()) in
		let body = cpBodyNew mass moment in
		let () = cpBodySetPos body (cpv center_x center_y) in
		let () = cpSpaceAddBody space body in
		let shape = cpPolyShapeNew body verts (cpvzero()) in
		let () = cpShapeSetElasticity shape 0.0 in
		let () = cpShapeSetFriction shape 1.5 in
		let () = cpShapeSetCollisionType shape 0 in
		let () = cpSpaceAddShape space shape in
		(body, size)
	
	let show (body, size) = 
		let rot = cpBodyGetRot ~body in
		Types.Data.List[
			Types.Data.String "Box";
			Types.Data.Float (cpBodyGetPos ~body).cp_x;
			Types.Data.Float (cpBodyGetPos ~body).cp_y;
			Types.Data.Float size;
			Types.Data.Float rot.cp_x;
			Types.Data.Float rot.cp_y]

	let draw data = match data with
		| Types.Data.List [
			Types.Data.String "Box";
			Types.Data.Float c_x;
			Types.Data.Float c_y;
			Types.Data.Float size;
			Types.Data.Float rot_x;
			Types.Data.Float rot_y] ->
				let size = size *. Config.cell_size *. 0.5 in
				let verts = genVerts c_x c_y size (cpv ~x:rot_x ~y:rot_y) in
				let () = GL.glBegin GL.GL_LINE_LOOP in
				let () = GL.glColor3 1.0 1.0 1.0 in
				let () = 
					for ii = 0 to pred 4 do
						GL.glVertex2 verts.(ii).cp_x verts.(ii).cp_y
					done 
				in GL.glEnd()
		| _ -> assert false

	let step (body, size) = ()
end

type t = Segment of Segment.t
	   | Box of Box.t

let show obj = match obj with
	| Segment segment -> Segment.show segment
	| Box box -> Box.show box

let draw data = match data with
	| Types.Data.List (Types.Data.String "Segment" :: _) -> 
		Segment.draw data
	| Types.Data.List (Types.Data.String "Box" :: _) -> 
		Box.draw data
	| data -> failwith ("Don't know how to draw: " ^ Types.Data.show data)

let step obj = match obj with
	| Segment segment -> Segment.step segment
	| Box box -> Box.step box

